#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/string.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <zbar.h>
#include "origincar_msg/msg/sign.hpp"
#include <vector>
#include <std_msgs/msg/int32.hpp>

using namespace std::chrono_literals;

class CvDetection : public rclcpp::Node {
public:
    CvDetection() : Node("cv_detection"), qr_scanner_() {
        // 初始化参数
        up = 120;
        left = 50;
        area = 1200;
        right_park = 640;
        up_park = 270;
        task = 0;

        // 配置ZBar只识别QR码
        qr_scanner_.set_config(zbar::ZBAR_QRCODE, zbar::ZBAR_CFG_ENABLE, 1);

        // 订阅者
        sign4return_sub_ = create_subscription<std_msgs::msg::Int32>(
            "/sign4return", 10,
            [this](const std_msgs::msg::Int32::SharedPtr msg) { sign4return_callback(msg); });
        
        // 订阅未压缩图像
        image_sub_ = create_subscription<sensor_msgs::msg::Image>(
            "/image_raw", 3,  // 确保订阅的是原始图像
            [this](const sensor_msgs::msg::Image::SharedPtr msg) { image_callback(msg); });

        // 发布者
        cone_pub_ = create_publisher<std_msgs::msg::Int32MultiArray>("/cone", 10);
        park_pub_ = create_publisher<std_msgs::msg::Int32MultiArray>("/park", 10);
        qrcode_pub_ = create_publisher<std_msgs::msg::String>("/qrcode", 10);
        sign_pub_ = create_publisher<origincar_msg::msg::Sign>("sign_switch", 10);
    }

private:
    void sign4return_callback(const std_msgs::msg::Int32::SharedPtr msg) {
        if (msg->data == 6) {
            task = 3;
            task3_time = now();
        } else if (msg->data == -1) {
            task = 1;
        } else if (msg->data == 5) {
            task = 2;
        }
    }

    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
        // 使用cv_bridge直接转换未压缩图像
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
            return;
        }
        cv::Mat frame = cv_ptr->image;

        if (task == 1) {
            auto cone_pos = cone_frame(frame);
            auto cone_msg = std_msgs::msg::Int32MultiArray();
            cone_msg.data = {cone_pos[0], cone_pos[1], cone_pos[2], cone_pos[3]};
            cone_pub_->publish(cone_msg);

            decode_qr_code(frame);
        }

        if (task == 3) {
            auto dt = (now() - task3_time).nanoseconds() / 1e9;
            if (dt < 4.0) {
                auto cone_pos = cone_frame(frame);
                auto cone_msg = std_msgs::msg::Int32MultiArray();
                cone_msg.data = {cone_pos[0], cone_pos[1], cone_pos[2], cone_pos[3]};
                cone_pub_->publish(cone_msg);
            } else {
                auto park_pos = park_frame(frame);
                auto park_msg = std_msgs::msg::Int32MultiArray();
                park_msg.data = {park_pos[0], park_pos[1], park_pos[2], park_pos[3]};
                park_pub_->publish(park_msg);
            }
        }
    }

    std::array<int, 4> cone_frame(cv::Mat& frame) {
        // 缩小ROI区域
        cv::Rect roi_rect(left, up, 550, 330); // width=550, height=330
        cv::Mat roi = frame(roi_rect);
        
        cv::Mat hsv, mask;
        cv::cvtColor(roi, hsv, cv::COLOR_BGR2HSV);
        
        // 合并颜色阈值条件
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(82, 255, 255), mask);
        cv::Mat mask2;
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(255, 90, 255), mask2);
        cv::bitwise_or(mask, mask2, mask);

        // 形态学降噪
        cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, {5,5}));

        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(255 - mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

        cv::Rect largest_rect;
        int max_area = area;
        for (auto& cnt : contours) {
            auto rect = cv::boundingRect(cnt);
            int rect_area = rect.area();
            if (rect_area > max_area) {
                max_area = rect_area;
                largest_rect = rect;
                // 找到足够大的区域后提前退出
                if (rect_area > 3000) break;
            }
        }

        return {
            int(left + largest_rect.x + largest_rect.width/2),
            up + largest_rect.y + largest_rect.height,
            largest_rect.width,
            largest_rect.height
        };
    }

    void decode_qr_code(cv::Mat& frame) {
        cv::Mat gray;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
        zbar::Image image(gray.cols, gray.rows, "Y800", gray.data, gray.cols * gray.rows);
        
        if (qr_scanner_.scan(image)) {
            for (auto s = image.symbol_begin(); s != image.symbol_end(); ++s) {
                std::string data = s->get_data();
                if (data == "ClockWise" || data == "AntiClockWise") {
                    publish_sign_message(data);
                    return;
                }
            }
        }
    }

    // 其他保持不变的函数...

    // 成员变量
    zbar::ImageScanner qr_scanner_;
    // 其他成员变量...
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CvDetection>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}